// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.

#pragma once

#include <aliceVision/feature/imageDescriberCommon.hpp>
#include <aliceVision/image/pixelTypes.hpp>
#include <aliceVision/numeric/numeric.hpp>
#include <aliceVision/stl/FlatMap.hpp>
#include <aliceVision/types.hpp>
#include <aliceVision/sfmData/Observation.hpp>

namespace aliceVision {
namespace sfmData {

/**
 * @brief Landmark is a 3D point with its 2d observations.
 */
class Landmark
{
  public:
    Landmark() {}

    explicit Landmark(feature::EImageDescriberType descType)
      : descType(descType)
    {}

    Landmark(const Vec3& pos3d,
             feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED,
             const image::RGBColor& color = image::WHITE)
      : X(pos3d),
        descType(descType),
        rgb(color)
    {}

    Vec3 X = { 0.0, 0.0, 0.0 };
    feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED;
    image::RGBColor rgb = image::WHITE;  //!> the color associated to the point
    EEstimatorParameterState state = EEstimatorParameterState::REFINED;
    IndexT referenceViewIndex = UndefinedIndexT;

    bool operator==(const Landmark& other) const
    {
        return AreVecNearEqual(X, other.X, 1e-3) 
        && AreVecNearEqual(rgb, other.rgb, 1e-3) 
        && _observations == other._observations 
        && descType == other.descType
        && referenceViewIndex == other.referenceViewIndex;
    }

    inline bool operator!=(const Landmark& other) const { return !(*this == other); }

    const Observations& getObservations() const { return _observations; }

    Observations& getObservations() { return _observations; }

    MapObservations getMapObservations() const 
    { 
        return MapObservations(_observations.begin(), _observations.end());;
    }
    
    inline void updateFromEstimator(const std::array<double, 3> & data)
    {
        // do not update a landmark set as Ignored or Constant in the Local strategy
        if (state != EEstimatorParameterState::REFINED)
        {
            return;
        }

        for (std::size_t i = 0; i < 3; ++i)
        {
            X(Eigen::Index(i)) = data.at(i);
        }
    }

    /**
     * @brief Is this landmark robustness independent of parallax
     * @return true if this landmark has this special property
    */
    bool isParallaxRobust() const { return _parallaxRobust; }

    /**
    * @brief decide if this landmark is robust even if its parallax is low
    * @param parallaxRobust True if robust to lack of parallax
    */
    void setParallaxRobust(bool parallaxRobust) { _parallaxRobust = parallaxRobust; }

    /**
     * @brief Is this landmark precisely located
     * @return true if this landmark has this special property
    */
    bool isPrecise() const { return _isPrecise; }

    /**
    * @brief decide if this landmark is robust even if its parallax is low
    * @param isPrecise True if robust to lack of parallax
    */
    void setIsPrecise(bool isPrecise) { _isPrecise = isPrecise; }

private:
    Observations _observations;
    bool _parallaxRobust = false;
    bool _isPrecise = false;
};

}  // namespace sfmData
}  // namespace aliceVision
